
#include <plan_manage/ego_replan_fsm.h>

#define PI 3.1415926
#define yaw_error_max 25.0/180*PI

namespace ego_planner
{

  void EGOReplanFSM::init(ros::NodeHandle &nh)
  {
    current_wp_ = 0;
    exec_state_ = FSM_EXEC_STATE::INIT;
    have_target_ = false;
    have_odom_ = false;

    /*  fsm param  */
    nh.param("fsm/flight_type", target_type_, -1);
    nh.param("fsm/thresh_replan", replan_thresh_, -1.0);
    nh.param("fsm/thresh_no_replan", no_replan_thresh_, -1.0);
    nh.param("fsm/planning_horizon", planning_horizen_, -1.0);
    nh.param("fsm/planning_horizen_time", planning_horizen_time_, -1.0);
    nh.param("fsm/emergency_time_", emergency_time_, 1.0);
    nh.param("fsm/w_adjust_", w_adjust, 1.0);

    /* initialize main modules */
    visualization_.reset(new PlanningVisualization(nh));
    planner_manager_.reset(new EGOPlannerManager);
    planner_manager_->initPlanModules(nh, visualization_);
    dir = POSITIVE;

    /* callback */
    exec_timer_ = nh.createTimer(ros::Duration(0.01), &EGOReplanFSM::execFSMCallback, this);
    safety_timer_ = nh.createTimer(ros::Duration(0.05), &EGOReplanFSM::checkCollisionCallback, this);

    odom_sub_ = nh.subscribe("/odom_map", 1, &EGOReplanFSM::odometryCallback, this);

    bspline_pub_ = nh.advertise<ego_planner::Bspline>("/planning/bspline", 10);
    data_disp_pub_ = nh.advertise<ego_planner::DataDisp>("/planning/data_display", 100);
    odom_adjust_pub_ = nh.advertise<nav_msgs::Odometry>("/odom_adjust",100);
    dir_pub = nh.advertise<std_msgs::UInt8>("/direction",100);
    stop_pub = nh.advertise<std_msgs::UInt8>("/emergency_stop",100);
    adjust_pose_pub_ = nh.advertise<ego_planner::PoseAdjust>("/adjust_pose", 10);

    // kkcqh change
    need_replan_sub_ = nh.subscribe("/kk/need_replan", 1, &EGOReplanFSM::NeedReplanCallback, this);
    start_bypass_sub_ = nh.subscribe("/kk/start_bypass", 1, &EGOReplanFSM::StartPathCallback, this);
    dispatch_sub_ = nh.subscribe("/kk/dispatch", 1, &EGOReplanFSM::RequestPathCallback, this);
    traj_server_twist_sub_ = nh.subscribe("/inner_algorithm_cmd_vel", 50, &EGOReplanFSM::TrajServerTwistCallback, this);

    control_back_pub_ = nh.advertise<planning_msgs::ControlBack>("/planning_back", 50);

    end_collision_check_timer_conter_.reset();
    
  }

void EGOReplanFSM::goal_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    end_pt_ << msg->pose.position.x, msg->pose.position.y, odom_pos_(2);

    // geometry_msgs/Quaternion to yaw
    tf::Quaternion tq(
        msg->pose.orientation.x,
        msg->pose.orientation.y,
        msg->pose.orientation.z,
        msg->pose.orientation.w);
    tf::Matrix3x3 tm(tq);
    double tr, tp, ty;
    tm.getRPY(tr, tp, ty);
    kk_end_yaw_ = ty;

    bool success = false;

    success = planner_manager_->planGlobalTraj(odom_pos_,odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());

    visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);

    if (success)
    {
        /*** display ***/
        constexpr double step_size_t = 0.1;
        int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
        vector<Eigen::Vector3d> gloabl_traj(i_end);
        for (int i = 0; i < i_end; i++)
        {
            gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t);
        }
        end_vel_.setZero();
        have_target_ = true;
        have_new_target_ = true;

        //goal is too close to current pose

        /*** FSM ***/
        if (exec_state_ == WAIT_TARGET)
        {
            changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
        }
        else if (exec_state_ == EXEC_TRAJ)
        {
            changeFSMExecState(REPLAN_TRAJ, "TRIG");
        }

        visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);

        // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(1, 0, 0, 1), 0.3, 0);
        //visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
    }
    else
    {
        ROS_ERROR("Unable to generate global trajectory!");
    }
}


  void EGOReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg)
  {
    odom_pos_(0) = msg->pose.pose.position.x;
    odom_pos_(1) = msg->pose.pose.position.y;
    odom_pos_(2) = msg->pose.pose.position.z;

    odom_vel_(0) = msg->twist.twist.linear.x;
    odom_vel_(1) = msg->twist.twist.linear.y;
    odom_vel_(2) = msg->twist.twist.linear.z;

    //odom_acc_ = estimateAcc( msg );

    odom_orient_.w() = msg->pose.pose.orientation.w;
    odom_orient_.x() = msg->pose.pose.orientation.x;
    odom_orient_.y() = msg->pose.pose.orientation.y;
    odom_orient_.z() = msg->pose.pose.orientation.z;

    tf::quaternionMsgToTF(msg->pose.pose.orientation,quat);
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    if(dir==NEGATIVE)
    {
        if(yaw>0)
        {
            yaw -= PI;
        }else if(yaw<0)
        {
            yaw += PI;
        }
    }
      nav_msgs::Odometry odom_adjust;
      geometry_msgs::Quaternion quat=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
      odom_adjust = *msg;
      odom_adjust.pose.pose.orientation = quat;
      odom_adjust_pub_.publish(odom_adjust);

    have_odom_ = true;
  }

  void EGOReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call)
  {

    if (new_state == exec_state_)
      continously_called_times_++;
    else
      continously_called_times_ = 1;

    static string state_str[7] = {"INIT", "WAIT_TARGET","ADJUST_POSE","GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP"};
    int pre_s = int(exec_state_);
    exec_state_ = new_state;
    cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl;
  }

  void EGOReplanFSM::checkYawError() {
      yaw_error = yaw_start-yaw;
      //first step : find the real yaw error
      if(abs(yaw_error)>yaw_error_max)
      {
          //if yaw error is larger than PI,it means the symbol between current yaw and target yaw is different
          if(abs(yaw_error)>PI)
          {
              //calculate the real yaw error with symbol
              yaw_error = yaw_error - yaw_error/abs(yaw_error)*2*PI;
              //if real yaw error larger than PI/2, change the direction of robot
              if(abs(yaw_error)>PI/2)
              {
                  changeDirection();

              }
          }
          else
          {
              cmd_vel.linear.x = 0;
              cmd_vel.angular.z = yaw_error/abs(yaw_error)*w_adjust;

          }
      }
  }

  double EGOReplanFSM::calculateYawError(double yaw_cur,double yaw_target) {
      double error = yaw_target - yaw_cur;
      if(abs(error)>PI)
      {
          error = error - error/abs(error)*2*PI;
          return error;
      }
      else
      {
          return error;
      }
  }

  void EGOReplanFSM::changeDirection() {
      if(dir == POSITIVE)
      {
          dir = NEGATIVE;
      }else
      {
          dir = POSITIVE;
      }
      std_msgs::UInt8 dir_new;
      dir_new.data = dir;
      dir_pub.publish(dir_new);
  }
  bool EGOReplanFSM::CheckPoseValid(double thre_angle) {
    double thre_radins = thre_angle * M_PI / 180.0;
    double desired_yaw = std::fmod(kk_target_yaw_, 2 * M_PI);
    double angle_diff = std::atan2(std::sin(yaw - desired_yaw), std::cos(yaw - desired_yaw));
    if (std::abs(angle_diff) > thre_radins) {
      return false;
    } else {
      return true;
    }
  }

  std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> EGOReplanFSM::timesOfConsecutiveStateCalls()
  {
    return std::pair<int, FSM_EXEC_STATE>(continously_called_times_, exec_state_);
  }

  void EGOReplanFSM::printFSMExecState()
  {
    static string state_str[7] = {"INIT", "WAIT_TARGET","ADJUST_POSE","GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP"};

    cout << "[FSM]: state: " + state_str[int(exec_state_)] << endl;
  }

  void EGOReplanFSM::execFSMCallback(const ros::TimerEvent &e) {

    static int fsm_num = 0;
    fsm_num++;
    if (fsm_num == 500)
    {
      printFSMExecState();
      if (!have_odom_)
        cout << "no odom." << endl;
      else 
        planner_manager_->grid_map_->CheckAndUpdateMapOrigin();
      fsm_num = 0;
    }

    if (have_target_) {
      // 在规划过程中，实时发布control back话题
      if (fsm_num % 5 == 0) {
        PubAControlBack(1);
      }
    }

    switch (exec_state_)
    {
    case INIT:
    {
      if (!have_odom_)
      {
        return;
      }
      changeFSMExecState(WAIT_TARGET, "FSM");
      break;
    }

    case WAIT_TARGET:
    {
      // if (!have_target_)
      //   return;
      // else
      // {
      //   changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      // }
      return;
      break;
    }

    case ADJUST_POSE: {
      // 1. check if the pose is valid
      ego_planner::PoseAdjust pa_msg;
      pa_msg.target_yaw = kk_target_yaw_;
      
      if (!CheckPoseValid()) {
        pa_msg.need_to_adjust = true;
        adjust_pose_pub_.publish(pa_msg);
        return;
      } else {
        pa_msg.need_to_adjust = false;
        adjust_pose_pub_.publish(pa_msg);
        
        if (end_adjust) {
          end_adjust = false;
          have_target_ = false;
          changeFSMExecState(WAIT_TARGET, "FSM");
          PubAControlBack(0);
          return;
        }
        std_msgs::UInt8 stop_cmd;
        stop_cmd.data = 0;
        stop_pub.publish(stop_cmd);
        auto info = &planner_manager_->local_data_;
        info->start_time_ = ros::Time::now();
        publishBspline();
        changeFSMExecState(EXEC_TRAJ, "FSM");
      }
      break;
    }

    case GEN_NEW_TRAJ:
    {
      start_pt_ = odom_pos_;
      start_vel_ << 0,0,0;
      start_acc_.setZero();

      // Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
      // start_yaw_(0)         = atan2(rot_x(1), rot_x(0));
      // start_yaw_(1) = start_yaw_(2) = 0.0;

      bool flag_random_poly_init;
      if (timesOfConsecutiveStateCalls().first == 1)
        flag_random_poly_init = false;
      else
        flag_random_poly_init = true;

      bool success = callReboundReplan(true, flag_random_poly_init);
      if (success)
      {
          Eigen::Vector3d vel_start = planner_manager_->local_data_.velocity_traj_.evaluateDeBoor(0.1);
          yaw_start = atan2(vel_start(1),vel_start(0));
          cout<<"yaw start : "<<yaw_start<<endl;
          yaw_error = yaw_start-yaw;
          kk_target_yaw_ = yaw_start;
          if (!CheckPoseValid()) {
            changeFSMExecState(ADJUST_POSE, "FSM");
            last_state_ = GEN_NEW_TRAJ;
            return;
          }

        auto info = &planner_manager_->local_data_;
        info->start_time_ = ros::Time::now();
        publishBspline();
        changeFSMExecState(EXEC_TRAJ, "FSM");
        // flag_escape_emergency_ = true;
      }
      else
      {
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      }
      break;
    }

    case REPLAN_TRAJ:
    {

      if (planFromCurrentTraj())
      {
        // check 该路径是否碰撞
        /* ---------- check trajectory ---------- */
        LocalTrajData *info = &planner_manager_->local_data_;
        if (checkTrajCollision(info)) {
          changeFSMExecState(REPLAN_TRAJ, "SAFETY");
          return;
        }
          Eigen::Vector3d vel_start = planner_manager_->local_data_.velocity_traj_.evaluateDeBoor(0.1);
          yaw_start = atan2(vel_start(1),vel_start(0));
          yaw_error = yaw_start-yaw;

          kk_target_yaw_ = yaw_start;
          if (!CheckPoseValid(60)) {
            changeFSMExecState(ADJUST_POSE, "FSM");
            last_state_ = REPLAN_TRAJ;
            return;
          }

          // auto info = &planner_manager_->local_data_;
          info->start_time_ = ros::Time::now();
          std_msgs::UInt8 stop_cmd;
          stop_cmd.data = 0;
          stop_pub.publish(stop_cmd);
          publishBspline();
          changeFSMExecState(EXEC_TRAJ, "FSM");
      }
      else
      {
          changeFSMExecState(REPLAN_TRAJ, "FSM");
      }

      break;
    }

    case EXEC_TRAJ:
    {
      /* determine if need to replan */
      LocalTrajData *info = &planner_manager_->local_data_;
      ros::Time time_now = ros::Time::now();
      double t_cur = (time_now - info->start_time_).toSec();
      t_cur = min(info->duration_, t_cur);

      Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);
      Eigen::Vector2d cur_pos_2d(odom_pos_(0), odom_pos_(1));
      Eigen::Vector2d end_pt_2d(end_pt_(0), end_pt_(1));
      Eigen::Vector3d traj_end_pt_3d = info->position_traj_.evaluateDeBoor(info->duration_);
      Eigen::Vector2d traj_end_pt_2d(traj_end_pt_3d(0), traj_end_pt_3d(1));

      // 是否进行重规划 
      if (need_replan_) {
        need_replan_ = false;
        // 检查是否到达目标点
        cout << "[Debug] ent pt is " << end_pt_.transpose() << endl;
        cout << "[Debug] traj end pt is " << traj_end_pt_2d.transpose() << endl;
        if ((traj_end_pt_2d-end_pt_2d).norm() < 0.3) {
          kk_target_yaw_ = kk_end_yaw_;
          // 终点朝向检查
          if (CheckPoseValid()) {
            have_target_ = false;
            changeFSMExecState(WAIT_TARGET, "FSM");
            PubAControlBack(0);
            return;
          } else {
            changeFSMExecState(ADJUST_POSE, "FSM");
            end_adjust = true;
            return;
          }
        }
        // 未到达目的地，重规划
        changeFSMExecState(REPLAN_TRAJ, "FSM");
      }

      /* && (end_pt_ - pos).norm() < 0.5 */
      // if (t_cur > info->duration_ - 1e-2)
      // {
      //   have_target_ = false;

      //   changeFSMExecState(WAIT_TARGET, "FSM");
      //   return;
      // }
      // else if ((end_pt_ - pos).norm() < no_replan_thresh_)
      // {
      //   // cout << "near end" << endl;
      //   return;
      // }
      // else if ((info->start_pos_ - pos).norm() < replan_thresh_)
      // {
      //   // cout << "near start" << endl;
      //   return;
      // }
      // else
      // {
      //   changeFSMExecState(REPLAN_TRAJ, "FSM");
      // }
      break;
    }

    case EMERGENCY_STOP:
    {

      // if (flag_escape_emergency_) // Avoiding repeated calls
      // {
      //   callEmergencyStop(odom_pos_);
      // }
      // else
      // {
      //   if (odom_vel_.norm() < 0.1)
      //     changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      // }

      // flag_escape_emergency_ = false;
      break;
    }
    }

    data_disp_.header.stamp = ros::Time::now();
    data_disp_pub_.publish(data_disp_);
  }

  bool EGOReplanFSM::planFromCurrentTraj()
  {

    LocalTrajData *info = &planner_manager_->local_data_;
    ros::Time time_now = ros::Time::now();
    double t_cur = (time_now - info->start_time_).toSec();

    //cout << "info->velocity_traj_=" << info->velocity_traj_.get_control_points() << endl;

    start_pt_ = odom_pos_;//info->position_traj_.evaluateDeBoorT(t_cur);
    start_vel_ = Eigen::Vector3d::Zero();//info->velocity_traj_.evaluateDeBoorT(t_cur);
    start_acc_ = Eigen::Vector3d::Zero();//info->acceleration_traj_.evaluateDeBoorT(t_cur);

    yaw_start = atan2((end_pt_-odom_pos_)(1),(end_pt_-odom_pos_)(0));
    yaw_error = yaw_start-yaw;

    bool success = callReboundReplan(false, false);

    if (!success)
    {
      success = callReboundReplan(true, false);
      //changeFSMExecState(EXEC_TRAJ, "FSM");
      if (!success)
      {
        success = callReboundReplan(true, true);
        if (!success)
        {
          return false;
        }
      }
    }

    return true;
  }

  void EGOReplanFSM::checkCollisionCallback(const ros::TimerEvent &e)
  {
    LocalTrajData *info = &planner_manager_->local_data_;
    auto map = planner_manager_->grid_map_;

    kk_emergency_stop_ = false;

    if (exec_state_ == ADJUST_POSE) {
      // 自转只检测当前是否被占据
      Eigen::Vector2d cur_pose_2d(odom_pos_(0), odom_pos_(1));
      if (map->getInflateOccupancy2d(cur_pose_2d)) {
        kk_emergency_stop_ = true;
      }
      return;
    }

    if (exec_state_ == EXEC_TRAJ) {
      // 0. 检测终点是否被占据，若被占据，急停
      Eigen::Vector2d pos_end2d(end_pt_(0), end_pt_(1));
      if (map->getInflateOccupancy2d(pos_end2d)) {
        end_collision_check_timer_conter_.start();
        kk_emergency_stop_ = true;
        ROS_WARN_STREAM("[DEBUG] end point is occupied, emergency stop! Continue " << end_collision_check_timer_conter_.getTotalDuration().toSec() << "s");
        // 进入下一个目标点逻辑
        if (end_collision_check_timer_conter_.getTotalDuration().toSec() > 5.0) {
          IntoNextDispatch();
        }
        return;
      } else {
        end_collision_check_timer_conter_.reset();
      }
      // 1. 检测当前轨迹是否被占据
      /* ---------- check trajectory ---------- */
      if (checkTrajCollision(info))
        changeFSMExecState(REPLAN_TRAJ, "SAFETY");
      // constexpr double time_step = 0.01;
      // double t_cur = (ros::Time::now() - info->start_time_).toSec();
      // for (double t = 0.0; t < info->duration_; t += time_step) {
      //   Eigen::Vector3d pos_cur = info->position_traj_.evaluateDeBoorT(t);
      //   Eigen::Vector2d pos_cur2d;
      //   pos_cur2d << pos_cur(0),pos_cur(1);
      //   if (map->getInflateOccupancy2d(pos_cur2d)) {
      //     changeFSMExecState(REPLAN_TRAJ, "SAFETY");
      //     return;
      //   }
      // }
      return;
    }

    kk_emergency_stop_ = true;
    return;
  }

  bool EGOReplanFSM::callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj)
  {

    getLocalTarget();
    start_pt_(2) = odom_pos_(2);
    start_vel_(2) = 0;
    start_acc_(2) = 0;
    local_target_pt_(2) = odom_pos_(2);
    local_target_vel_(2) = 0;

    bool plan_success =
        planner_manager_->reboundReplan(start_pt_, start_vel_, start_acc_, local_target_pt_, local_target_vel_, (have_new_target_ || flag_use_poly_init), flag_randomPolyTraj);
    have_new_target_ = false;

    // cout << "final_plan_success=" << plan_success << endl;

    if (plan_success)
    {
      auto info = &planner_manager_->local_data_;
      //publishBspline();
      Eigen::MatrixXd control_points = info->position_traj_.get_control_points();
      for(int i=0;i<control_points.cols();i++) control_points.col(i)(2) = odom_pos_(2);
      visualization_->displayOptimalList(control_points, 0);
    }

    return plan_success;
  }

  bool EGOReplanFSM::callEmergencyStop(Eigen::Vector3d stop_pos)
  {

    planner_manager_->EmergencyStop(stop_pos);

    auto info = &planner_manager_->local_data_;

    /* publish traj */
    ego_planner::Bspline bspline;
    bspline.order = 3;
    bspline.start_time = info->start_time_;
    bspline.traj_id = info->traj_id_;

    Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
    bspline.pos_pts.reserve(pos_pts.cols());
    for (int i = 0; i < pos_pts.cols(); ++i)
    {
      geometry_msgs::Point pt;
      pt.x = pos_pts(0, i);
      pt.y = pos_pts(1, i);
      pt.z = pos_pts(2, i);
      bspline.pos_pts.push_back(pt);
    }

    Eigen::VectorXd knots = info->position_traj_.getKnot();
    bspline.knots.reserve(knots.rows());
    for (int i = 0; i < knots.rows(); ++i)
    {
      bspline.knots.push_back(knots(i));
    }

    bspline_pub_.publish(bspline);

    return true;
  }

  void EGOReplanFSM::getLocalTarget() {
    // 如果起点已经很接近终点，直接使用终点
    double start_to_end_dist = (end_pt_ - start_pt_).norm();
    if (start_to_end_dist < planning_horizen_) {
      local_target_pt_ = end_pt_;
      local_target_vel_ = Eigen::Vector3d::Zero();
      return;
    }
    
    // 二分搜索找到目标点
    double t_start = planner_manager_->global_data_.last_progress_time_;
    double t_end = planner_manager_->global_data_.global_duration_;
    double t_mid, dist_mid;
    double dist_min = 9999, dist_min_t = t_start;
    
    // 二分查找大致位置
    while (t_end - t_start > 0.05) { // 精度阈值
      t_mid = (t_start + t_end) / 2;
      Eigen::Vector3d pos_mid = planner_manager_->global_data_.getPosition(t_mid);
      dist_mid = (pos_mid - start_pt_).norm();
      
      if (dist_mid < planning_horizen_)
        t_start = t_mid;
      else
        t_end = t_mid;
        
      // 更新最小距离
      if (dist_mid < dist_min) {
        dist_min = dist_mid;
        dist_min_t = t_mid;
      }
    }
    
    // 取目标点和速度
    local_target_pt_ = planner_manager_->global_data_.getPosition(t_end);
    planner_manager_->global_data_.last_progress_time_ = dist_min_t;
    
    // 终点附近减速逻辑
    if ((end_pt_ - local_target_pt_).norm() < (planner_manager_->pp_.max_vel_ * planner_manager_->pp_.max_vel_) / (2 * planner_manager_->pp_.max_acc_))
      local_target_vel_ = Eigen::Vector3d::Zero();
    else
      local_target_vel_ = planner_manager_->global_data_.getVelocity(t_end);
  }

  void EGOReplanFSM::publishBspline() {

      auto info = &planner_manager_->local_data_;
      info->start_time_ = ros::Time::now();
      /* publish traj */
      ego_planner::Bspline bspline;
      bspline.order = 3;
      bspline.start_time = info->start_time_;
      bspline.traj_id = info->traj_id_;

      Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
      //cout<<"optimal point : "<<endl<<pos_pts<<endl;
      bspline.pos_pts.reserve(pos_pts.cols());
      Eigen::Vector3d point_temp;
      for (int i = 0; i < pos_pts.cols(); ++i)
      {
          geometry_msgs::Point pt;
          pt.x = pos_pts(0, i);
          pt.y = pos_pts(1, i);
          pt.z = odom_pos_(2);
          bspline.pos_pts.push_back(pt);
          point_temp<<pt.x,pt.x,pt.x;
          //cout<<"point : "<<point_temp<<endl;
      }

      Eigen::VectorXd knots = info->position_traj_.getKnot();
      bspline.knots.reserve(knots.rows());
      for (int i = 0; i < knots.rows(); ++i)
      {
          bspline.knots.push_back(knots(i));
      }

      bspline_pub_.publish(bspline);
  }

  void EGOReplanFSM::NeedReplanCallback(const std_msgs::BoolConstPtr &msg) {
    need_replan_ = msg->data;
  }

// 调度适配函数
void EGOReplanFSM::RequestPathCallback(const forklift_msgs::PathSequence::ConstPtr &msg) {
  forklift_msgs::DirectionPath dp_msg = msg->paths[0];
  // 存储调度路径终点的朝向和坐标及调度名字
  reserve_dispatch_data_.name = dp_msg.path_name;
  ROS_INFO_STREAM("Get dispatch path, reserve it...; Path name is: " << dp_msg.path_name);
  /// 1. 判断是否为可绕行路径
  if (dp_msg.plan_boundary_left == 0.0 && dp_msg.plan_boundary_right == 0.0) {
    reserve_dispatch_data_.can_bypass = false;
    return;
  }
  reserve_dispatch_data_.can_bypass = true;
  /// 2. 存储路径终点
  geometry_msgs::Pose end_pose = dp_msg.path_segment.poses.back().pose;
  reserve_dispatch_data_.end_pt = Eigen::Vector3d(end_pose.position.x, end_pose.position.y, odom_pos_(2));
  /// 3. 存储路径终点yaw信息
  geometry_msgs::Pose before_end_pose = (*(dp_msg.path_segment.poses.end()-2)).pose;
  Eigen::Vector3d before_end_pose_v3d(before_end_pose.position.x, before_end_pose.position.y, odom_pos_(2));
  Eigen::Vector3d direction = reserve_dispatch_data_.end_pt - before_end_pose_v3d;
  // 计算yaw角度（弧度制）
  double end_yaw = std::atan2(direction(1), direction(0));
  // 存储终点的yaw值
  reserve_dispatch_data_.end_yaw = end_yaw;
}

void EGOReplanFSM::StartPathCallback(const forklift_msgs::PathSequence::ConstPtr &msg) {
  if (exec_state_ != WAIT_TARGET) {
    ROS_WARN_STREAM("System state invalid!");
    return;
  }
  forklift_msgs::DirectionPath dp_msg = msg->paths[0];
  // 存储调度路径终点的朝向和坐标及调度名字
  current_dispatch_data_.name = dp_msg.path_name;
  ROS_INFO_STREAM("Get start path; Path name is: " << dp_msg.path_name);
  /// 1. 存储路径终点
  geometry_msgs::Pose end_pose = dp_msg.path_segment.poses.back().pose;
  current_dispatch_data_.end_pt = Eigen::Vector3d(end_pose.position.x, end_pose.position.y, odom_pos_(2));
  /// 2. 存储路径终点yaw信息
  geometry_msgs::Pose before_end_pose = (*(dp_msg.path_segment.poses.end()-2)).pose;
  Eigen::Vector3d before_end_pose_v3d(before_end_pose.position.x, before_end_pose.position.y, odom_pos_(2));
  Eigen::Vector3d direction = current_dispatch_data_.end_pt - before_end_pose_v3d;
  ROS_INFO_STREAM("direction is " << direction.transpose());
  // 计算yaw角度（弧度制）
  double end_yaw = std::atan2(direction(1), direction(0));
  // 存储终点的yaw值
  current_dispatch_data_.end_yaw = end_yaw;
  /// 3. 尝试清空预留数据
  if (current_dispatch_data_.name == reserve_dispatch_data_.name) {
    reserve_dispatch_data_.can_bypass = false;
    reserve_dispatch_data_.name = "NOTHING";
  }
  /// 4. 更新原goal callback逻辑
  UpdateNextGoal();
}

void EGOReplanFSM::TrajServerTwistCallback(const geometry_msgs::Twist::ConstPtr &msg) {
  traj_server_twist_data_ = *msg;
}

void EGOReplanFSM::PubAControlBack(int type) {
  planning_msgs::ControlBack msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "world";

  msg.result = type;
  if (!kk_emergency_stop_)
    msg.twist = traj_server_twist_data_;
  control_back_pub_.publish(msg);
}

void EGOReplanFSM::IntoNextDispatch() {
  if (reserve_dispatch_data_.name == "NOTHING" || reserve_dispatch_data_.can_bypass == false) {
    ROS_WARN_STREAM("No reserve dispatch data! or Next path cant bypass");
    return;
  }
  ROS_INFO_STREAM("Into next dispatch!");
  current_dispatch_data_ = reserve_dispatch_data_;
  reserve_dispatch_data_.name = "NOTHING";
  reserve_dispatch_data_.can_bypass = false;
  // 存储调度路径终点的朝向和坐标及调度名字
  if (UpdateNextGoal()) {
    changeFSMExecState(GEN_NEW_TRAJ, "IntoNextDispatch");
    kk_emergency_stop_ = false;
  }
}

bool EGOReplanFSM::UpdateNextGoal() {
  end_pt_ = current_dispatch_data_.end_pt;
  kk_end_yaw_ = current_dispatch_data_.end_yaw;
  ROS_INFO_STREAM("Current target info, ent pt is " << end_pt_.transpose() << "; end yaw is " << kk_end_yaw_);

  /// 5. 生成一个从起点到终点的平滑参考轨迹，不考虑障碍物仅提供大致方向指导
  bool success = false;
  success = planner_manager_->planGlobalTraj(odom_pos_,odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());

  // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);

  if (success) {
    /*** display ***/
    // constexpr double step_size_t = 0.1;
    // int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
    // vector<Eigen::Vector3d> gloabl_traj(i_end);
    // for (int i = 0; i < i_end; i++)
    // {
    //     gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t);
    // }
    end_vel_.setZero();
    have_target_ = true;
    have_new_target_ = true;

    //goal is too close to current pose

    /*** FSM ***/
    if (exec_state_ == WAIT_TARGET) {
      changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
    }
    //todo 暂时不支持中途添加新的目标点
    // else if (exec_state_ == EXEC_TRAJ) {
    //   changeFSMExecState(REPLAN_TRAJ, "TRIG");
    // }

    // visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
  }
  else {
    ROS_ERROR("Unable to generate global trajectory!");
  }
  return success;
}

// 检查轨迹碰撞，返回true表示有碰撞
bool EGOReplanFSM::checkTrajCollision(LocalTrajData* info, double check_horizon) {
  constexpr double time_step = 0.01;
  auto map = planner_manager_->grid_map_;
  
  // 如果check_horizon为负数，则检查整个轨迹
  double check_duration = (check_horizon < 0) ? info->duration_ : std::min(check_horizon, info->duration_);
  
  for (double t = 0.0; t < check_duration; t += time_step) {
    Eigen::Vector3d pos_cur = info->position_traj_.evaluateDeBoorT(t);
    Eigen::Vector2d pos_cur2d(pos_cur(0), pos_cur(1));
    if (map->getInflateOccupancy2d(pos_cur2d)) {
      return true; // 发现碰撞
    }
  }
  
  return false; // 无碰撞
}

} // namespace ego_planner
